#include <ros/ros.h>

#include <sensor_msgs/Imu.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/Temperature.h>

#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/Pose.h>

#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Char.h>
#include <visualization_msgs/MarkerArray.h>

#include "sunray_viobot/algo_ctrl.h"
#include "sunray_viobot/algo_status.h"
#include "sunray_viobot/viobot_ctrl.h"

//打印算法状态
void viobot_algo_status_cb(const sunray_viobot::algo_status::ConstPtr &msg)
{
    std::cout << "algo_status: " << msg->algo_status << std::endl;
}

int main(int argc, char **argv)
{

    ros::init(argc, argv, "viobot_demo");
    ros::NodeHandle nh("~");

    // 订阅 /viobot 节点发布的话题
    //ros::Subscriber viobot_sys_status_sub = nh.subscribe("/viobot/sys_status", 1, viobot_sys_status_cb);
    ros::Subscriber viobot_algo_status_sub = nh.subscribe("/viobot/algo_status", 1, viobot_algo_status_cb);
    //ros::Subscriber viobot_camera_left_info_sub = nh.subscribe("/viobot/camera_left_info", 1, viobot_camera_left_info_cb);
    //ros::Subscriber viobot_camera_right_info_sub = nh.subscribe("/viobot/camera_right_info", 1, viobot_camera_right_info_cb);
    //ros::Subscriber viobot_image_left_sub = nh.subscribe("/viobot/image_left", 1, viobot_image_left_cb);
    //ros::Subscriber viobot_image_right_sub = nh.subscribe("/viobot/image_right", 1, viobot_image_right_cb);
    //ros::Subscriber viobot_imu_sub = nh.subscribe("/viobot/imu", 1, viobot_imu_cb);
    //ros::Subscriber viobot_imu_temp_sub = nh.subscribe("/viobot/imu_temp", 1, viobot_imu_temp_cb);
    //ros::Subscriber viobot_pcd_save_sub = nh.subscribe("/viobot/pr_loop/PCD_Save", 1, viobot_pcd_save_cb);
    //ros::Subscriber viobot_global_consistency_sub = nh.subscribe("/viobot/pr_loop/global_consistency", 1, viobot_global_consistency_cb);
    //ros::Subscriber viobot_keyframe_action_cancel_sub = nh.subscribe("/viobot/pr_loop/keyframe_action/cancel", 1, viobot_keyframe_action_cancel_cb);
    //ros::Subscriber viobot_keyframe_action_goal_sub = nh.subscribe("/viobot/pr_loop/keyframe_action/goal", 1, viobot_keyframe_action_goal_cb);
    //ros::Subscriber viobot_reset_vocabulary_sub = nh.subscribe("/viobot/reset_vocabulary", 1, viobot_reset_vocabulary_cb);
    //ros::Subscriber viobot_stereo2_restart_sub = nh.subscribe("/viobot/stereo2/restart", 1, viobot_stereo2_restart_cb);

    // 订阅 /stereo2 节点发布的话题
    //ros::Subscriber stereo2_base_path_sub = nh.subscribe("/viobot/pr_loop/base_path", 1, stereo2_base_path_cb);
    //ros::Subscriber stereo2_camera_pose_visual_sub = nh.subscribe("/viobot/pr_loop/camera_pose_visual", 1, stereo2_camera_pose_visual_cb);
    //ros::Subscriber stereo2_keyframe_action_feedback_sub = nh.subscribe("/viobot/pr_loop/keyframe_action/feedback", 1, stereo2_keyframe_action_feedback_cb);
    //ros::Subscriber stereo2_keyframe_action_result_sub = nh.subscribe("/viobot/pr_loop/keyframe_action/result", 1, stereo2_keyframe_action_result_cb);
    //ros::Subscriber stereo2_keyframe_action_status_sub = nh.subscribe("/viobot/pr_loop/keyframe_action/status", 1, stereo2_keyframe_action_status_cb);
    //ros::Subscriber stereo2_match_image_sub = nh.subscribe("/viobot/pr_loop/match_image", 1, stereo2_match_image_cb);
    //ros::Subscriber stereo2_odometry_adjusted_sub = nh.subscribe("/viobot/pr_loop/odometry_adjusted", 1, stereo2_odometry_adjusted_cb);
    //ros::Subscriber stereo2_odometry_loop_sub = nh.subscribe("/viobot/pr_loop/odometry_loop", 1, stereo2_odometry_loop_cb);
    //ros::Subscriber stereo2_odometry_rect_sub = nh.subscribe("/viobot/pr_loop/odometry_rect", 1, stereo2_odometry_rect_cb);
    //ros::Subscriber stereo2_points_sub = nh.subscribe("/viobot/pr_loop/points", 1, stereo2_points_cb);
    //ros::Subscriber stereo2_points_adjust_sub = nh.subscribe("/viobot/pr_loop/points_adjust", 1, stereo2_points_adjust_cb);
    //ros::Subscriber stereo2_pose_graph_sub = nh.subscribe("/viobot/pr_loop/pose_graph", 1, stereo2_pose_graph_cb);
    //ros::Subscriber stereo2_pose_graph_path_sub = nh.subscribe("/viobot/pr_loop/pose_graph_path", 1, stereo2_pose_graph_path_cb);
    //ros::Subscriber stereo2_rdf_points_sub = nh.subscribe("/viobot/pr_loop/rdf_points", 1, stereo2_rdf_points_cb);
    //ros::Subscriber stereo2_camera_pose_sub = nh.subscribe("/viobot/stereo2/camera_pose", 1, stereo2_camera_pose_cb);
    //ros::Subscriber stereo2_feature_img_sub = nh.subscribe("/viobot/stereo2/feature_img", 1, stereo2_feature_img_cb);
    //ros::Subscriber stereo2_imu_propagate_sub = nh.subscribe("/viobot/stereo2/imu_propagate", 1, stereo2_imu_propagate_cb);
    //ros::Subscriber stereo2_odometry_sub = nh.subscribe("/viobot/stereo2/odometry", 1, stereo2_odometry_cb);
    //ros::Subscriber stereo2_odometry_path_sub = nh.subscribe("/viobot/stereo2/odometry_path", 1, stereo2_odometry_path_cb);

    ros::spin();
     
    return 0;
}
